/* USER CODE BEGIN Header */
/*
_ooOoo_
o8888888o
88" . "88
(| -_- |)
 O\ = /O
___/`---'\____
.   ' \\| |// `.
/ \\||| : |||// \
/ _||||| -:- |||||- \
| | \\\ - /// | |
| \_| ''\---/'' | |
\ .-\__ `-` ___/-. /
___`. .' /--.--\ `. . __
."" '< `.___\_<|>_/___.' >'"".
| | : `- \`.;`\ _ /`;.`/ - ` : | |
\ \ `-. \_ __\ /__ _/ .-` / /
======`-.____`-.___\_____/___.-`____.-'======
`=---='

         .............................................
          佛曰：bug泛滥，我已瘫痪！
*/

/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2024 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "real_main.hpp"
#include "robot.hpp"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
void MX_FREERTOS_Init(void);
/* USER CODE BEGIN PFP */
using namespace DJI_Motor_n;
DM_Motor_n::DM_Mit_Mode_c *dm_p = nullptr;
 DJI_Motor_n::DJI_Motor_Instance *p_m = nullptr;
 DJI_Motor_n::DJI_Motor_Instance *p_l = nullptr;

 float test1 = 100;
 /* USER CODE END PFP */

 /* Private user code ---------------------------------------------------------*/
 /* USER CODE BEGIN 0 */

 /* USER CODE END 0 */

 /**
  * @brief  The application entry point.
  * @retval int
  */
 int main(void)
 {

     /* USER CODE BEGIN 1 */

     /* USER CODE END 1 */

     /* MCU Configuration--------------------------------------------------------*/

     /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
     HAL_Init();

     /* USER CODE BEGIN Init */

     /* USER CODE END Init */

     /* Configure the system clock */
     SystemClock_Config();

     /* USER CODE BEGIN SysInit */

     /* USER CODE END SysInit */

     /* Initialize all configured peripherals */
     MX_GPIO_Init();
     MX_DMA_Init();
     MX_CAN1_Init();
     MX_CAN2_Init();
     MX_USART1_UART_Init();
     MX_USART3_UART_Init();
     MX_USART2_UART_Init();
     MX_SPI1_Init();
     MX_USART6_UART_Init();
     MX_TIM2_Init();

     /* USER CODE BEGIN 2 */
     //测试代码
     // BMI088Instance_c bmi088_test;
     // while (bmi088_test.BMI088Init(&hspi1, 1) != BMI088_NO_ERROR)
     //     ;
     // /* USER CODE END 2 */
     // while (1)
     // {
     //     bmi088_test.BMI088_Read(&bmi088_test.BMI088);
     //     value = bmi088_test.BMI088;
     // }

    //  Motor_General_Def_n::Motor_Init_Config_s friction_l = {
    //      .controller_param_init_config = {
    //          .speed_PID = {
    //              .Kp = 12.2f,
    //              .Ki = 0.0f,
    //              .Kd = 3.0f,
    //              .ActualValueSource = NULL,
    //              .mode = Output_Limit | Deadzone,
    //              .max_out = 5000,
    //              .deadband = 0.3,
    //          },
    //      },
    //      .controller_setting_init_config = {
    //          .outer_loop_type = Motor_General_Def_n::SPEED_LOOP, // 使用速度环
    //          .close_loop_type = Motor_General_Def_n::SPEED_LOOP,
    //          .motor_reverse_flag = Motor_General_Def_n::MOTOR_DIRECTION_NORMAL,
    //          .feedback_reverse_flag = Motor_General_Def_n::FEEDBACK_DIRECTION_NORMAL,
    //          // .angle_feedback_source = Motor_General_Def_n::MOTOR_FEED,
    //          .speed_feedback_source = Motor_General_Def_n::MOTOR_FEED, // 电机反馈
    //          //  .feedforward_flag = Motor_General_Def_n::FEEDFORWARD_NONE,
    //      },
    //      .motor_type = Motor_General_Def_n::GM6020,
    //      .can_init_config = {
    //          .can_handle = &hcan1, // 使用can2
    //          .tx_id = 4,           // 看电调闪几下就填几
    //      }};
    //  Motor_General_Def_n::Motor_Init_Config_s friction_r = {
    //      .controller_param_init_config = {
    //          .speed_PID = {
    //              .Kp = 7.5f,
    //              .Ki = 0.0f,
    //              .Kd = 0.1f,
    //              .ActualValueSource = NULL,
    //              .mode = Output_Limit | Deadzone,
    //              .max_out = 5000,
    //              .deadband = 0.3,
    //          },
    //      },
    //      .controller_setting_init_config = {
    //          .outer_loop_type = Motor_General_Def_n::SPEED_LOOP, // 使用速度环
    //          .close_loop_type = Motor_General_Def_n::SPEED_LOOP,
    //          .motor_reverse_flag = Motor_General_Def_n::MOTOR_DIRECTION_NORMAL,
    //          .feedback_reverse_flag = Motor_General_Def_n::FEEDBACK_DIRECTION_NORMAL,
    //          // .angle_feedback_source = Motor_General_Def_n::MOTOR_FEED,
    //          .speed_feedback_source = Motor_General_Def_n::MOTOR_FEED, // 电机反馈
    //          //  .feedforward_flag = Motor_General_Def_n::FEEDFORWARD_NONE,
    //      },
    //      .motor_type = Motor_General_Def_n::GM6020,
    //      .can_init_config = {
    //          .can_handle = &hcan2, // 使用can2
    //          .tx_id = 4,           // 看电调闪几下就填几
    //      }};
    //   p_m = new DJI_Motor_n::DJI_Motor_Instance(friction_l);
    //   p_l = new DJI_Motor_n::DJI_Motor_Instance(friction_r);

    //   p_m->DJIMotorEnable();
    //   p_l->DJIMotorEnable();
    //   p_m->DJIMotorSetRef(1000);
    //   p_l->DJIMotorSetRef(1000);

      // dm_p->StateSet(DM_Motor_n::DM_PORTECT_ZERO_POSITION);
      /*************************************************************************/
       robot_init();
      /* USER CODE END 2 */
    
      /* Call init function for freertos objects (in cmsis_os2.c) */
       MX_FREERTOS_Init();

      // /* Start scheduler */
       osKernelStart();

      /* We should never get here as control is now taken by the scheduler */
      /* Infinite loop */
      /* USER CODE BEGIN WHILE */
      while (1)
      {
          /* USER CODE END WHILE */
          // dm_p->Transmit(DM_p_out, DM_v, DM_kp, DM_kd, 0);
          //  p_m->DJIMotorSetRef(test1);
          DJI_Motor_n::DJIMotorControl();
          /* USER CODE BEGIN 3 */
     }
     /* USER CODE END 3 */
}
/**
 * @brief System Clock Configuration
 * @retval None
 */
void SystemClock_Config(void)
{
    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

    /** Configure the main internal regulator output voltage
     */
    __HAL_RCC_PWR_CLK_ENABLE();
    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

    /** Initializes the RCC Oscillators according to the specified parameters
     * in the RCC_OscInitTypeDef structure.
     */
    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
    RCC_OscInitStruct.HSEState = RCC_HSE_ON;
    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
    RCC_OscInitStruct.PLL.PLLM = 4;
    RCC_OscInitStruct.PLL.PLLN = 168;
    RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
    RCC_OscInitStruct.PLL.PLLQ = 7;
    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
    {
        Error_Handler();
    }

    /** Initializes the CPU, AHB and APB buses clocks
     */
    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
    {
        Error_Handler();
    }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
 * @brief  Period elapsed callback in non blocking mode
 * @note   This function is called  when TIM7 interrupt took place, inside
 * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
 * a global variable "uwTick" used as application time base.
 * @param  htim : TIM handle
 * @retval None
 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    /* USER CODE BEGIN Callback 0 */

    /* USER CODE END Callback 0 */
    if (htim->Instance == TIM7)
    {
        HAL_IncTick();
    }
    /* USER CODE BEGIN Callback 1 */

    /* USER CODE END Callback 1 */
}

/**
 * @brief  This function is executed in case of error occurrence.
 * @retval None
 */
void Error_Handler(void)
{
    /* USER CODE BEGIN Error_Handler_Debug */
    /* User can add his own implementation to report the HAL error return state */
    __disable_irq();
    while (1)
    {
    }
    /* USER CODE END Error_Handler_Debug */
}

#ifdef USE_FULL_ASSERT
/**
 * @brief  Reports the name of the source file and the source line number
 *         where the assert_param error has occurred.
 * @param  file: pointer to the source file name
 * @param  line: assert_param error line source number
 * @retval None
 */
void assert_failed(uint8_t *file, uint32_t line)
{
    /* USER CODE BEGIN 6 */
    /* User can add his own implementation to report the file name and line number,
       ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
    /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
